#include "servo.h"

uint16_t puls_length[20]; // array for all delays

/************************************************************************

   void setservo(byte index, byte value)

   Set servo position
   value: 0..255

 ***************************************************************************/

void set_servo(byte index, byte value)
{
	uint16_t wert;
	int multi = 256;

	wert=MINPULS+(MAXPULS-MINPULS)/multi*value; //

	// callculate hightime
	puls_length[index<<1]=0-wert;

	// sume of low and hightime for one servo is 2ms
	puls_length[(index<<1)+1]=0-(TIMER_MAXPULS-wert);

	// 10 servos give you 10*2ms=20ms total cycle time
	return;
}

/**
 * Sets the direction of the servo motor - values allowed are
 * in between SERVO_1_STEER_MAX_LEFT and SERVO_1_STEER_MAX_RIGHT
 */
void set_servo_1_direction(byte dir){
	if(dir < SERVO_1_STEER_MAX_LEFT)
		dir = SERVO_1_STEER_MAX_LEFT;
	else if(dir > SERVO_1_STEER_MAX_RIGHT)
		dir = SERVO_1_STEER_MAX_RIGHT;

	set_servo(0,dir);
	return;
}
